#include "Wander.h"

#include <stdlib.h>
#include <math.h>
#include <iostream>

using namespace std;

Wander::Wander(){
  wanderOffset        = 0.0001;
  wanderRadius        = 0.00006;
  wanderRate          = 0.001;
  wanderOrientation   = randomBinomial();
  maxAcceleration     = 100;
  cout << "construyo el wonder" << endl;
}

Steering* Wander::getSteering(){
  // cout << "getSteering del wonder" << endl;
  // this->updateKinematicData();
  
  float targetOrientation;
  float aux[3];
  wanderOrientation += randomBinomial()*wanderRate;
  targetOrientation = wanderOrientation + character->orientation;
  multVectorScalar2(asVector(character->orientation),wanderOffset,aux);
  if (target == NULL)
    target = new Kinematic();
  sumarVectores(character->position,aux,target->position);
  multVectorScalar2(asVector(targetOrientation),wanderRadius,aux);
  sumarVectores(target->position,aux,target->position);
  Steering* ret = Face::getSteering();
  multVectorScalar2(asVector(character->orientation),maxAcceleration,ret->linear);
  return ret;
}

float Wander::randomBinomial(){
  float ret = ((float) (rand() - rand()))/RAND_MAX;
  // DEBUG
//    cout << "randomBinomial devolvio" << ret << "." << endl;
  return ret; 
}
